#include <assert.h>
#ifndef PHIDGET_ALREADY
#define PHIDGET_ALREADY
#include "phidget21.h"
#endif


#define RACK_INDEX 1

#define RACK_HIGHEST_POINT 100.0

#define RACK_ACCELERATION 1000.0
#define RACK_VELOCITY 16.0

#define PINION_CIRCUMFERENCE 439.8
#define MM_PER_DEGREE (PINION_CIRCUMFERENCE/360.0)


class RackController {
	
public:
	CPhidgetAdvancedServoHandle servo;

	void init(CPhidgetAdvancedServoHandle servoIn) {
		servo = servoIn;
		CPhidgetAdvancedServo_setServoType(servo, RACK_INDEX, PHIDGET_SERVO_HITEC_HS422);
		CPhidgetAdvancedServo_setAcceleration(servo, RACK_INDEX, RACK_ACCELERATION);
		CPhidgetAdvancedServo_setVelocityLimit(servo, RACK_INDEX, RACK_VELOCITY);
	}

	// set the target position to the current position so it doesn't move when we engage
	void engage() {
		double pos;
		CPhidgetAdvancedServo_getPosition (servo, RACK_INDEX, &pos);
		CPhidgetAdvancedServo_setPosition (servo, RACK_INDEX, pos);
		CPhidgetAdvancedServo_setEngaged(servo, RACK_INDEX, 1);;
	}

	// set degrees directly
	void setByDegrees(double degrees) {
		CPhidgetAdvancedServo_setPosition(servo, RACK_INDEX, degrees);
	}

	// move rack position
	void incrementByMm(double mm) {
		double pos;
		CPhidgetAdvancedServo_getPosition (servo, RACK_INDEX, &pos);
		assert(pos + mm/MM_PER_DEGREE < 190.0);
		CPhidgetAdvancedServo_setPosition (servo, RACK_INDEX, pos + mm/MM_PER_DEGREE);
	}

	void setByMm(double mm) {

		// TODO: fix this

		// false << "is this plus or minus??";
		// CPhidgetAdvancedServo_setPosition (servo, RACK_INDEX, RACK_HIGHEST_POINT + (mm/MM_PER_DEGREE));
	}

	void disengage() {
		CPhidgetAdvancedServo_setEngaged(servo, RACK_INDEX, 0);
	}

};